#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/nodes/SoSeparator.h>
#include <Inventor/nodes/SoIndexedFaceSet.h>
#include <Inventor/nodes/SoCoordinate3.h>

#include <assert.h>
#include <iostream>
#include <list>
#include <vector>
#include <math.h>

#include "halfedges.h"
#include "vec3f.h"

HE_edge::HE_edge(int _vert, HE_face *_face) : vert(_vert), pair(NULL), face(_face), next(NULL), prev(NULL) {
}

HE_edge::HE_edge() : vert(-1), pair(NULL), face(NULL), next(NULL), prev(NULL) {
}

HE_face::HE_face() : edge(NULL), size(0), tip(NULL) {
}

Mesh::Mesh() : edges(), faces(), verts() {
}

Mesh::~Mesh() {
	for (vector<HE_face *>::iterator p=faces.begin(); p != faces.end(); p++) {
		delete (HE_face *)(*p);
	}
	for (vector<HE_edge *>::iterator p=edges.begin(); p != edges.end(); p++) {
		delete (HE_edge *)(*p);
	}
}

const float phi = (sqrt(5)+1.0)/2.0;

void Mesh::pair_edges() {
	vector<list<HE_edge *> > vert_edges(verts.size());
//	int cnt2 = 0;
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *edge = face->tip;
		do {
			assert(edge->next);
			int a = edge->vert;
			int b = edge->next->vert;
			// search b's edge list for edge going to a
			// assign half edge as pair
			list<HE_edge *>::iterator p2 = vert_edges[b].begin();
			for (; p2 != vert_edges[b].end(); p2++) {
				HE_edge *edge2 = *p2;
				assert(edge2);
				// found edge?
				if (edge2->vert!=a) continue;
				// link edges
				edge->next->pair = edge2;
				edge2->pair = edge->next;
//				cnt2++;
				// remove edge and stop
				vert_edges[b].erase(p2);
				break;
			}
			// if not found and not paired, add edge to the vert's list
			if (vert_edges[b].end()==p2 && !edge->next->pair) {
				vert_edges[a].push_back(edge->next);
			}
			edge = edge->next;
		} while (edge!=face->tip);
	}
//	int cnt = 0;
//	for (size_t i=0; i<vert_edges.size(); i++) {
//			cnt+= vert_edges[i].size();
//		for (list<HE_edge *>::iterator p = vert_edges[i].begin(); p!=vert_edges[i].end(); p++) {
//		}
//	}
//	cerr << cnt << " " << cnt2 << endl;
}


bool Mesh::subdivide(vector<vector<float> > coeffs) {
	if (faces[0]->type==HE_face::thick || faces[0]->type==HE_face::thin) {
		return tt2dk(coeffs);
	} else {
		return dk2tt(coeffs);
	}
	return true;
}

bool Mesh::tt2dk(vector<vector<float> > coeffs) {
	vector<int> idxverts2(faces.size(), -1);
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		if (face->type==HE_face::kite || face->type==HE_face::dart) return false;
		assert(face && face->tip && face->edge && 4==face->size);
		if (HE_face::thick!=face->type) continue;
//		float l0 = 1-2*(1-1/(phi*phi));//coeffs[7][0];
//		float l2 = 0;//l0-1+2/phi;
//		float l1 = 1-1/(phi*phi);//1/phi-l2;
		float l0 = coeffs[7][0];
		float l2 = l0-1+2/phi;
		float l1 = 1/phi-l2;


//		cerr << l0 << " " << l1 << " " << l2 << " " << (l0+2*l1+l2) << endl;
		float point[3] = {0,0,0};
		for (int k=0; k<3; k++) {
			point[k] = l0*verts[face->tip->vert*3+k]+
			           l1*verts[face->tip->next->vert*3+k]+
			           l1*verts[face->tip->prev->vert*3+k]+
			           l2*verts[face->tip->next->next->vert*3+k];
		}
		idxverts2[p-faces.begin()] = verts.size()/3;
		for (int k=0; k<3; k++) {
			verts.push_back(point[k]);
		}
	}
	vector<HE_edge *> newedges;
	vector<HE_face *> newfaces;
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *edge = (HE_face::thick==face->type?face->tip->next:face->tip);
		if (HE_face::thick==face->type) {
			{
				//dart
				HE_face *k1 = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[2] = new HE_edge(idxverts2[p-faces.begin()], k1);
				newe[1] = new HE_edge(edge->next->next->vert, k1);
				newe[0] = new HE_edge(edge->next->vert, k1);
				newe[3] = new HE_edge(edge->vert, k1);
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				k1->tip  = newe[0];
				k1->edge = newe[0];
				k1->size = 4;
				k1->type = HE_face::dart;
				newfaces.push_back(k1);
			}
			if (edge->pair) {
				//kite
				HE_face *d = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[0] = new HE_edge(edge->vert, d);
				newe[1] = new HE_edge(idxverts2[p-faces.begin()], d);
				newe[2] = new HE_edge(edge->prev->vert, d);
				if (HE_face::thick==edge->pair->face->type) {
					int z = -1;
					for (vector<HE_face *>::iterator p2=faces.begin(); p2!=faces.end(); p2++) {
						if (*p2==edge->pair->face) {z = p2-faces.begin(); break;}
					}
					assert(z>=0);
					newe[3] = new HE_edge(idxverts2[z], d);
				} else {
					newe[3] = new HE_edge(edge->pair->next->vert, d);
				}
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				d->tip  = newe[0];
				d->edge = newe[0];
				d->size = 4;
				d->type = HE_face::kite;
				newfaces.push_back(d);
			}
		} else {
			if (edge->pair) {
				{
				//kite
				HE_face *d = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[0] = new HE_edge(edge->vert, d);
				newe[1] = new HE_edge(edge->next->next->vert, d);
				newe[2] = new HE_edge(edge->prev->vert, d);
				if (HE_face::thick==edge->pair->face->type) {
					int z = -1;
					for (vector<HE_face *>::iterator p2=faces.begin(); p2!=faces.end(); p2++) {
						if (*p2==edge->pair->face) {z = p2-faces.begin(); break;}
					}
					assert(z>=0);
					newe[3] = new HE_edge(idxverts2[z], d);
				} else {
					newe[3] = new HE_edge(edge->pair->next->vert, d);
				}
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				d->tip  = newe[0];
				d->edge = newe[0];
				d->size = 4;
				newfaces.push_back(d);
				d->type = HE_face::kite;
				}
			}
		}
	}
	int stats[] = {0,0,0,0,0,0,0,0,0};
	vector<float> replverts;
	vector<int> irverts;
	vector<bool> vmark(verts.size(), false);
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *fedge = face->tip;
		do {
			if (!vmark[fedge->vert]) {
				HE_edge *edge = fedge;
				bool interior = true;
				int cnt = 0;
				int nthick = 0;
				int nthin = 0;
				do {
					if (HE_face::thick==edge->face->type) {
						nthick++;
					} else {
						nthin++;
					}
					if (edge->pair) {
						edge = edge->pair->prev;
					} else {
						interior = false;
						vmark[fedge->vert] = true;
						break;
					}
					cnt++;
				} while (edge!=fedge);
				if (interior) {
					vmark[fedge->vert] = true;
					edge = fedge;
					float point[3] = {0,0,0};
					interior = false;
					if (5==nthick && 0==nthin) { //S or S5
						stats[(fedge->vert == face->tip->vert) ?7:4]++;
					} else if (2==nthick && 1==nthin) { //D
						stats[0]++;
						interior = true;
						do {
							if (HE_face::thin==edge->face->type) break;
							edge = edge->pair->prev;
						} while (edge!=fedge);

						float l2 = coeffs[9][0];//.125;
						float l3 = coeffs[9][1];//.125;
						float l4 = coeffs[9][2];//.125;
						float l1 = phi*l4 + (3-phi)*phi*l3-l2;
						float l0 = 1-l1-2*l2-2*l3-l4;
						for (int i=0; i<3; i++) {
							point[i] += l0*verts[edge->vert*3+i];
							point[i] += l1*verts[edge->prev->prev->vert*3+i];
							point[i] += l2*verts[edge->prev->vert*3+i];
							point[i] += l2*verts[edge->next->vert*3+i];
							point[i] += l3*verts[edge->pair->next->vert*3+i];
							point[i] += l3*verts[edge->next->pair->prev->prev->vert*3+i];
							point[i] += l4*verts[edge->next->pair->next->vert*3+i];
						}
					} else if (1==nthick && 2==nthin) { //Q
						stats[1]++;
					} else if (3==nthick && 1==nthin) { //K
						stats[2]++;
					} else if (3==nthick && 2==nthin) { //J
						stats[3]++;
					} else if (3==nthick && 4==nthin) { //S3
						stats[5]++;
					} else if (4==nthick && 2==nthin) { //S4
						stats[6]++;
					}   else {
//						cerr << nthick << " " << nthin << endl;
						stats[8]++;
					}
					if (interior) {
						irverts.push_back(fedge->vert);
						for (int i=0; i<3; i++) {
							replverts.push_back(point[i]);
						}
					}
				}
			}
			fedge = fedge->next;
		} while (fedge!=face->tip);
	}
	for (int i=0; i<(int)irverts.size(); i++) {
		for (int k=0; k<3; k++) {
			verts[irverts[i]*3+k] = replverts[i*3+k];
		}
	}

	for (vector<HE_face *>::iterator p=faces.begin(); p != faces.end(); p++) {
		delete (HE_face *)(*p);
	}
	for (vector<HE_edge *>::iterator p=edges.begin(); p != edges.end(); p++) {
		delete (HE_edge *)(*p);
	}
	faces = newfaces;
	edges = newedges;



//	cerr << "tt: ";
	int total = 0;
	for (int i=0; i<9; i++) {
		total += stats[i];
	}
	for (int i=0; i<9; i++) {
//		cerr << i << ":" <<  (float)stats[i]/(float)total << " ";
	}
//	cerr << endl;

	pair_edges();
	return true;
}

bool Mesh::dk2tt(vector<vector<float> > coeffs) {
	vector<int> idxverts2(faces.size(), -1);
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		if (face->type==HE_face::thick || face->type==HE_face::thin) return false;
		assert(face && face->tip && face->edge && 4==face->size);
		if (HE_face::kite!=face->type) continue;
		float l0 = coeffs[7][1];
		float l2 = (l0+1.)/(2*phi*phi-1);
		float l1 = 1-phi*phi*l2;
		float point[3] = {0,0,0};
		for (int k=0; k<3; k++) {
			point[k] = l0*verts[face->tip->vert*3+k]+
			           l1*verts[face->tip->next->vert*3+k]+
			           l1*verts[face->tip->prev->vert*3+k]+
			           l2*verts[face->tip->next->next->vert*3+k];
		}
		idxverts2[p-faces.begin()] = verts.size()/3;
		for (int k=0; k<3; k++) {
			verts.push_back(point[k]);
		}
	}

	vector<HE_edge *> newedges;
	vector<HE_face *> newfaces;
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *edge = (HE_face::kite==face->type?face->tip->next->next:face->tip->next);
		if (HE_face::kite==face->type) {
		{
				//thin
				HE_face *k1 = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[0] = new HE_edge(idxverts2[p-faces.begin()], k1);
				newe[3] = new HE_edge(edge->prev->vert, k1);
				newe[2] = new HE_edge(face->tip->vert, k1);
				newe[1] = new HE_edge(edge->next->vert, k1);
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				k1->tip  = newe[0];
				k1->edge = newe[0];
				k1->size = 4;
				k1->type = HE_face::thin;
				newfaces.push_back(k1);
			}
			if (edge->pair) {
				//thick
				HE_face *d = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[0] = new HE_edge(edge->prev->vert, d);
				newe[3] = new HE_edge(idxverts2[p-faces.begin()], d);
				newe[2] = new HE_edge(edge->vert, d);
				if (HE_face::kite==edge->pair->face->type) {
					int z = -1;
					for (vector<HE_face *>::iterator p2=faces.begin(); p2!=faces.end(); p2++) {
						if (*p2==edge->pair->face) {z = p2-faces.begin(); break;}
					}
					assert(z>=0);
					newe[1] = new HE_edge(idxverts2[z], d);
				} else {
					newe[1] = new HE_edge(edge->pair->next->next->vert, d);
				}
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				d->tip  = newe[0];
				d->edge = newe[0];
				d->size = 4;
				d->type = HE_face::thick;
				newfaces.push_back(d);
			}
		} else {
			if (edge->pair) {
				{
				//thick
				HE_face *d = new HE_face();
				HE_edge *newe[] = {NULL, NULL, NULL, NULL};
				newe[0] = new HE_edge(edge->prev->vert, d);
				newe[3] = new HE_edge(edge->next->vert, d);
				newe[2] = new HE_edge(edge->vert, d);
				if (HE_face::kite==edge->pair->face->type) {
					int z = -1;
					for (vector<HE_face *>::iterator p2=faces.begin(); p2!=faces.end(); p2++) {
						if (*p2==edge->pair->face) {z = p2-faces.begin(); break;}
					}
					assert(z>=0);
					newe[1] = new HE_edge(idxverts2[z], d);
				} else {
					newe[1] = new HE_edge(edge->pair->next->next->vert, d);
				}
				for (int z=0; z<4; z++) {
					newe[z]->next = newe[(z+1)%4];
					newe[z]->prev = newe[(z-1+4)%4];
					newedges.push_back(newe[z]);
				}
				d->tip  = newe[0];
				d->edge = newe[0];
				d->size = 4;
				newfaces.push_back(d);
				d->type = HE_face::thick;
				}
			}
		}
	}

	int stats[] = {0,0,0,0,0,0,0,0};
	vector<float> replverts;
	vector<int> irverts;
	vector<bool> vmark(verts.size(), false);
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *fedge = face->tip;
		do {
			if (!vmark[fedge->vert]) {
				HE_edge *edge = fedge;
				bool interior = true;
				int cnt = 0;
				int ndarts = 0;
				int nkites = 0;
				do {
					if (HE_face::dart==edge->face->type) {
						ndarts++;
					} else {
						nkites++;
					}
					if (edge->pair) {
						edge = edge->pair->prev;
					} else {
						interior = false;
						vmark[fedge->vert] = true;
						break;
					}
					cnt++;
				} while (edge!=fedge);
				if (interior) {
					interior = false;
					vmark[fedge->vert] = true;
					edge = fedge;
					float point[3] = {0,0,0};
					if (5==nkites) { //sun
						stats[2]++;
//						interior = true;
						float l0 = coeffs[2][0];
						float l1 = (1-l0)/10;
						do {
							for (int i=0; i<3; i++) {
								if (edge==fedge) {
									point[i] += l0*verts[edge->vert*3+i];
								}
								point[i] += l1*verts[edge->prev->vert*3+i];
								point[i] += l1*verts[edge->prev->prev->vert*3+i];
							}
							edge = edge->pair->prev;
						} while (edge!=fedge);
						//					cerr << "sun\n";
					} else if (5==ndarts) { //star
						stats[0]++;
										interior = true;
						float l1 = coeffs[0][0];
						float l2 = coeffs[0][1];
						float l0 = 1-(5*l1+5*l2);
						do {
							for (int i=0; i<3; i++) {
								if (edge==fedge) {
									point[i] += l0*verts[edge->vert*3+i];
								}
								point[i] += l1*verts[edge->prev->vert*3+i];
								point[i] += l2*verts[edge->prev->prev->vert*3+i];
							}
							edge = edge->pair->prev;
						} while (edge!=fedge);
						//					cerr << "star\n";
					} else if (1==ndarts && 2==nkites) {
						interior = true;
						stats[1]++;
						do {
							if (HE_face::dart==edge->face->type) break;
							edge = edge->pair->prev;
						} while (edge!=fedge);
						/*
						   float l0 = coeffs[0][0];
						   float l2 = l0-1+2/phi;
						   float l1 = 1/phi-l2;
						   for (int i=0; i<3; i++) {
						   point[i] += l0*verts[edge->pair->next->next->vert*3+i];
						   point[i] += l1*verts[edge->prev->vert*3+i];
						   point[i] += l1*verts[edge->next->vert*3+i];
						   point[i] += l2*verts[edge->next->next->vert*3+i];
						   }
						 */

						float l1 = coeffs[1][0];//.125;
						float l2 = coeffs[1][1];//.125;
						float l4 = coeffs[1][2];//.125;
						float l3 = (l4+l2/(phi*phi)-l1/phi)/(phi-3);
						float l0 = 1-l1-2*l2-2*l3-l4;
						for (int i=0; i<3; i++) {
							point[i] += l0*verts[edge->vert*3+i];
							point[i] += l1*verts[edge->prev->prev->vert*3+i];
							point[i] += l2*verts[edge->prev->vert*3+i];
							point[i] += l2*verts[edge->next->vert*3+i];
							point[i] += l3*verts[edge->pair->next->vert*3+i];
							point[i] += l3*verts[edge->next->pair->prev->prev->vert*3+i];
							point[i] += l4*verts[edge->next->pair->next->vert*3+i];
						}

						//					cerr << l0 << " " << l1 << " " << l2 << " " << l3 << " " << l4 << " "  << endl;
						//					cerr << "ace\n";
					} else if (2==ndarts && 3==nkites) {
						stats[4]++;
						//				interior = false;
						do {
							if (HE_face::dart==edge->pair->face->type && HE_face::dart==edge->next->pair->face->type) break;
							edge = edge->pair->prev;
						} while (edge!=fedge);

						float l1 = .045;
						float l2 = .125;
						float l4 = .125;
						float l3 = 0;
						float l5 = .0;

						float l6 = l1+l2/(phi*phi)+l3*(3-phi)+l4*(1-phi)-l5*phi;
						float l0 = 1-l1-2*l2-2*l3-2*l4 -2*l5 - l6;
						for (int i=0; i<3; i++) {
							point[i] += l0*verts[edge->vert*3+i];
							point[i] += l1*verts[edge->prev->prev->vert*3+i];
							point[i] += l2*verts[edge->prev->vert*3+i];
							point[i] += l2*verts[edge->next->vert*3+i];
							point[i] += l3*verts[edge->pair->next->vert*3+i];
							point[i] += l3*verts[edge->next->pair->prev->prev->vert*3+i];
							point[i] += l4*verts[edge->next->pair->next->vert*3+i];
							point[i] += l4*verts[edge->pair->next->next->vert*3+i];
							point[i] += l5*verts[edge->next->pair->next->pair->prev->prev->vert*3+i];
							point[i] += l5*verts[edge->pair->prev->pair->next->vert*3+i];
							point[i] += l6*verts[edge->pair->prev->pair->next->next->vert*3+i];
						}

						//					cerr << l0 << " " << l1 << " " << l2 << " " << l3 << " " << l4 << " " << l5 << " " << l6 << endl;
						//					cerr << "jack\n";
					} else if (3==ndarts && 2==nkites) {
						stats[3]++;
						//				interior = false;
						do {
							if (HE_face::dart==edge->pair->face->type && HE_face::dart==edge->next->pair->face->type) break;
							edge = edge->pair->prev;
						} while (edge!=fedge);

						float l1 = coeffs[3][0];
						float l2 = coeffs[3][1];
						float l3 = coeffs[3][2];
						float l4 = coeffs[3][3];
						float l5 = coeffs[3][4];
						float l6 = l1 + l2*phi*phi + l3/phi + l4*phi*(1-phi) - l5*phi*sqrt(5);
						float l0 = 1-l1-2*l2-2*l3-2*l4 -2*l5 - l6;
						for (int i=0; i<3; i++) {
							point[i] += l0*verts[edge->vert*3+i];
							point[i] += l1*verts[edge->prev->prev->vert*3+i];
							point[i] += l2*verts[edge->prev->vert*3+i];
							point[i] += l2*verts[edge->next->vert*3+i];
							point[i] += l3*verts[edge->pair->next->vert*3+i];
							point[i] += l3*verts[edge->next->pair->prev->prev->vert*3+i];
							point[i] += l4*verts[edge->next->pair->next->vert*3+i];
							point[i] += l4*verts[edge->pair->next->next->vert*3+i];
							point[i] += l5*verts[edge->next->pair->next->pair->prev->prev->vert*3+i];
							point[i] += l5*verts[edge->pair->prev->pair->next->vert*3+i];
							point[i] += l6*verts[edge->pair->prev->pair->next->next->vert*3+i];
						}

						//					cerr << l0 << " " << l1 << " " << l2 << " " << l3 << " " << l4 << " " << l5 << " " << l6 << endl;
						//					cerr << "king\n";
					} else if (1==ndarts && 4==nkites) {
						stats[5]++;
						//				interior = false;
						do {
							if (HE_face::dart==edge->face->type) break;
							edge = edge->pair->prev;
						} while (edge!=fedge);

						float l1 = .045;
						float l2 = .045;
						float l3 = .045;
						float l4 = .045;
						float l5 = .045;
						float l6 = l1/phi + l2*phi -l4/(phi*phi) + l5*(phi-3);
						float l0 = 1-l1-2*l2-2*l3-2*l4 -2*l5 - l6;
						for (int i=0; i<3; i++) {
							point[i] += l0*verts[edge->vert*3+i];
							point[i] += l1*verts[edge->prev->prev->vert*3+i];
							point[i] += l2*verts[edge->prev->vert*3+i];
							point[i] += l2*verts[edge->next->vert*3+i];
							point[i] += l3*verts[edge->pair->next->vert*3+i];
							point[i] += l3*verts[edge->next->pair->prev->prev->vert*3+i];
							point[i] += l4*verts[edge->next->pair->next->vert*3+i];
							point[i] += l4*verts[edge->pair->next->next->vert*3+i];
							point[i] += l5*verts[edge->next->pair->next->pair->prev->prev->vert*3+i];
							point[i] += l5*verts[edge->pair->prev->pair->next->vert*3+i];
							point[i] += l6*verts[edge->pair->prev->pair->next->next->vert*3+i];
						}

						//					cerr << l0 << " " << l1 << " " << l2 << " " << l3 << " " << l4 << " " << l5 << " " << l6 << endl;
						//					cerr << "queen\n";
					} else if (2==ndarts && 2==nkites) {
						//				interior = false;
						stats[6]++;
						do {
							if (HE_face::dart==edge->pair->face->type && HE_face::dart==edge->face->type) break;
							edge = edge->pair->prev;
						} while (edge!=fedge);

						float l1 = coeffs[6][0];
						float l2 = coeffs[6][1];
						float l4 = coeffs[6][2];
						float l5 = coeffs[6][3];
						float l3 = -l1+l2*(phi-3)+l4*(phi-1)+l5/phi;
						float l0 = 1-l1-l5-2*(l2+l3+l4);
						for (int i=0; i<3; i++) {
							point[i] += l0*verts[edge->vert*3+i];
							point[i] += l1*verts[edge->prev->vert*3+i];
							point[i] += l2*verts[edge->prev->prev->vert*3+i];
							point[i] += l2*verts[edge->pair->next->vert*3+i];
							point[i] += l3*verts[edge->next->vert*3+i];
							point[i] += l3*verts[edge->pair->prev->prev->vert*3+i];
							point[i] += l4*verts[edge->pair->prev->pair->next->vert*3+i];
							point[i] += l4*verts[edge->next->pair->next->next->vert*3+i];
							point[i] += l4*verts[edge->next->pair->next->vert*3+i];
						}

						//					cerr << l0 << " " << l1 << " " << l2 << " " << l3 << " " << l4 << " " << l5 << endl;
						//					cerr << "deuce\n";
					}  else {
						stats[7]++;
						//						cerr << "????????????" << endl;
						if (!ndarts || !nkites)
							interior = true;
						float l0 = coeffs[8][0];
						do {
							for (int i=0; i<3; i++) {
								if (edge==fedge) {
									point[i] += l0*verts[edge->vert*3+i];
								}
								point[i] += (1-l0)/(2.0*cnt)*verts[edge->prev->vert*3+i];
								point[i] += (1-l0)/(2.0*cnt)*verts[edge->prev->prev->vert*3+i];
							}
							edge = edge->pair->prev;
						} while (edge!=fedge);

					}
					if (interior) {
						irverts.push_back(fedge->vert);
						for (int i=0; i<3; i++) {
							replverts.push_back(point[i]);
						}
					}
				}
			}
			fedge = fedge->next;
		} while (fedge!=face->tip);
	}
	for (int i=0; i<(int)irverts.size(); i++) {
		for (int k=0; k<3; k++) {
			verts[irverts[i]*3+k] = replverts[i*3+k];
		}
	}



	for (vector<HE_face *>::iterator p=faces.begin(); p != faces.end(); p++) {
		delete (HE_face *)(*p);
	}
	for (vector<HE_edge *>::iterator p=edges.begin(); p != edges.end(); p++) {
		delete (HE_edge *)(*p);
	}
	faces = newfaces;
	edges = newedges;


	int total = 0;
	for (int i=0; i<8; i++) {
		total += stats[i];
	}
	for (int i=0; i<8; i++) {
//		cerr << i << ":" <<  (float)stats[i]/(float)total << " ";
	}
//	cerr << endl;

	pair_edges();
	return true;
}

float angle(Vector3f v21, Vector3f v23) {
	return atan2(Vector3f(v21^v23).norm(),v21*v23)*180./M_PI;
}

float angle(float x1, float y1, float z1, float x2, float y2, float z2, float x3, float y3, float z3) {
	Vector3f v21(x1-x2, y1-y2, z1-z2); // Vector from B to A
	Vector3f v23(x3-x2, y3-y2, z3-z2); // Vector from B to C
	return atan2(Vector3f(v21^v23).norm(),v21*v23)*180./M_PI;
}



std::ostream& operator<<(std::ostream& s, Mesh& he) {
	s << "Faces: " << he.faces.size() << "\n";
	// loop over faces
	for (vector<HE_face *>::iterator p=he.faces.begin(); p!=he.faces.end(); p++) {
		// loop over edges
		HE_face *face = *p;
		HE_edge *edge = face->edge;
		s << "Face: " << face->edge << " " << face->type  << " with the tip idx " << face->tip->vert << "\n";
//		bool dart = false;
		do {
			s << "Edge: " << edge << " "
				<< edge->vert << " ("
				<< he.verts[edge->vert*3  ] << ", "
				<< he.verts[edge->vert*3+1] << ", "
				<< he.verts[edge->vert*3+2] << ") "
				<< edge->face << " "
				<< edge->pair << " "
				<< "prev: " << edge->prev << " "
				<< "next: " << edge->next << "\n";
			edge = edge->next;
		} while (edge != face->edge);
//		if (dart) s << "it was a dart"<<endl;
	}
	return s;
}

bool Mesh::face_center(int idx, vector<float> &crd) {
	crd = vector<float>(3, 0.0);
	if (idx<0 || idx>=(int)faces.size()) return false;
	HE_edge *fedge = faces[idx]->tip;
	do {
		for (int i=0; i<3; i++) {
			crd[i] += verts[fedge->vert*3+i];
		}
		fedge = fedge->next;
	} while (fedge!=faces[idx]->tip);
	for (int i=0; i<3; i++) {
		crd[i] /= faces[idx]->size;
	}
	return true;
}

int Mesh::transformEdgesToVerts(HE_edge *edge, vector<list<pair<int, int> > > &edgeverts) {
	int a = edge->vert;
	int b = edge->prev->vert;

	int idx = -1;
	for (list<pair<int, int> >::iterator p2 = edgeverts[a].begin(); idx<0 && p2!=edgeverts[a].end(); p2++) {
		if ((*p2).first==b) {
			idx = (*p2).second;
		}
	}
	if (idx<0) {
		vector<float> crd(3, 0);
		for (int i=0; i<3; i++) {
			crd[i] = (verts[a*3+i] + verts[b*3+i])*.5;
		}
		idx = verts.size()/3;
		edgeverts[a].push_back(pair<int,int>(b, idx));
		edgeverts[b].push_back(pair<int,int>(a, idx));
		verts.insert(verts.end(), crd.begin(), crd.end());
	}
	assert(idx>=0);
	return idx;
}

bool Mesh::transform2penrose() {
	vector<int> faceverts(faces.size(), -1);
	vector<float> crd(3, 0);
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		faceverts[p-faces.begin()] = verts.size()/3;
		face_center(p-faces.begin(), crd);
		verts.insert(verts.end(), crd.begin(), crd.end());
	}

	vector<HE_edge *> newedges;
	vector<HE_face *> newfaces;
	vector<list<pair<int, int> > > edgeverts(verts.size() - faces.size(), list<pair<int, int> >());
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *edge = face->tip;
		do {
			int ceidx = transformEdgesToVerts(edge, edgeverts);
			int peidx = transformEdgesToVerts(edge->next, edgeverts);

			HE_face *thick = new HE_face();
			HE_edge *newe[] = {NULL, NULL, NULL, NULL};
			newe[0] = new HE_edge(faceverts[p-faces.begin()], thick);
			newe[1] = new HE_edge(ceidx, thick);
			newe[2] = new HE_edge(edge->vert, thick);
			newe[3] = new HE_edge(peidx, thick);

			for (int z=0; z<4; z++) {
				newe[z]->next = newe[(z+1)%4];
				newe[z]->prev = newe[(z-1+4)%4];
				newedges.push_back(newe[z]);
			}
			thick->tip  = newe[0];
			thick->edge = newe[0];
			thick->size = 4;
			newfaces.push_back(thick);
			thick->type = HE_face::thick;

			edge = edge->next;
		} while (edge!=face->tip);
	}

	for (vector<HE_face *>::iterator p=faces.begin(); p != faces.end(); p++) {
		delete (HE_face *)(*p);
	}
	for (vector<HE_edge *>::iterator p=edges.begin(); p != edges.end(); p++) {
		delete (HE_edge *)(*p);
	}
	faces = newfaces;
	edges = newedges;

	pair_edges();
	return true;
}

// Loads a single faceset into the data structure.
// The strategy is to loop over faces, inserting half edges
// for each edge, which are linked together, including a link to a new face.
// For convenience, this new face is itself stored in a member list.
// Additionally, the half edges are stored in lists
// at each vertex. Then these new half edges are looped over again
// to compare with the lists at each vertex to identify half-edge pairs.
// Identified pairs are then removed from the vertex's edge list.
// In this way each half edge is linked with it's next half edge,
// a face, a pair, and of course a single vertex.
#define ALREADY_DARTS_N_KITES 1
bool loadShape(SoIndexedFaceSet *shape, SoCoordinate3 *coords, Mesh &he) {
	for (vector<HE_face *>::iterator p=he.faces.begin(); p != he.faces.end(); p++) {
		delete (HE_face *)(*p);
	}
	for (vector<HE_edge *>::iterator p=he.edges.begin(); p != he.edges.end(); p++) {
		delete (HE_edge *)(*p);
	}
	he.edges = vector<HE_edge *>();
	he.faces = vector<HE_face *>();
	if (!shape || !coords || !shape->isOfType(SoIndexedFaceSet::getClassTypeId()) || !coords->isOfType(SoCoordinate3::getClassTypeId())) return false;

    int num_verts = coords->point.getNum();
	he.verts = vector<float>(num_verts*3);
	for (int i=0; i<num_verts; i++) {
		for (int j=0; j<3; j++) {
			he.verts[i*3+j] = coords->point[i][j];
		}
	}

    int num_idx   = shape->coordIndex.getNum();
	vector<list<HE_edge *> > vert_edges(num_verts);

	for (int j=0; j<num_idx; j++) { // loop over polygons, here j++ means skip -1 index
		HE_face *face = new HE_face();
		he.faces.push_back(face);
		HE_edge *prev_edge=NULL;
		for (; shape->coordIndex[j]>=0; j++) { // loop over vertices in the current polygon
			HE_edge *edge = new HE_edge(shape->coordIndex[j], face);
			he.edges.push_back(edge);
			face->size++;
			if (!face->edge) { // TODO delete ->edge, replace with ->tip
				face->edge = edge;
				face->tip = edge;
			} else {
				assert(prev_edge);
				prev_edge->next = edge;
//				edge->orig = prev_edge->vert; // for debugging purposes
			}
			edge->prev = prev_edge;
			prev_edge  = edge;
			// will be rewritten each time until the last edge
//			face->edge->orig = edge->vert;
			edge->next = face->edge;
			face->edge->prev = edge;
		}
		assert(face->edge);
#if ALREADY_DARTS_N_KITES

		// TODO ugly detection of darts
		HE_edge *edge = face->edge;
		bool dart = false;
		float a = 0;
		do {
			float x1 = he.verts[edge->vert*3  ];
			float y1 = he.verts[edge->vert*3+1];
			float z1 = he.verts[edge->vert*3+2];
			float x2 = he.verts[edge->next->vert*3  ];
			float y2 = he.verts[edge->next->vert*3+1];
			float z2 = he.verts[edge->next->vert*3+2];
			float x0 = he.verts[edge->prev->vert*3  ];
			float y0 = he.verts[edge->prev->vert*3+1];
			float z0 = he.verts[edge->prev->vert*3+2];

			Vector3f v10(x0-x1, y0-y1, z0-z1); // Vector from B to A
			Vector3f v12(x2-x1, y2-y1, z2-z1); // Vector from B to C
			Vector3f n = v10^v12;
			float b = angle(v10, v12);
			if (b>a || !face->tip) {
				face->tip = edge;
				a = b;
			}

			float x1p = he.verts[edge->prev->vert*3  ];
			float y1p = he.verts[edge->prev->vert*3+1];
			float z1p = he.verts[edge->prev->vert*3+2];
			float x2p = he.verts[edge->prev->next->vert*3  ];
			float y2p = he.verts[edge->prev->next->vert*3+1];
			float z2p = he.verts[edge->prev->next->vert*3+2];
			float x0p = he.verts[edge->prev->prev->vert*3  ];
			float y0p = he.verts[edge->prev->prev->vert*3+1];
			float z0p = he.verts[edge->prev->prev->vert*3+2];
			Vector3f v10p(x0p-x1p, y0p-y1p, z0p-z1p); // Vector from B to A
			Vector3f v12p(x2p-x1p, y2p-y1p, z2p-z1p); // Vector from B to C
			Vector3f np = v10p^v12p;
			if (angle(np,n)>90) {
				dart = true;
			}
			edge = edge->next;
		} while (edge != face->edge);
		if (dart) {
			face->tip = face->tip->next->next;
		}
		face->type = dart? HE_face::dart : HE_face::kite;
#endif
	}

	he.pair_edges();
#if !ALREADY_DARTS_N_KITES
	he.transform2penrose();
#endif

	return true;
}

#define SEND_TRIANGLES_INSTEAD_OF_FULL_FACES 1
bool unloadShape(SoIndexedFaceSet *faceset, SoCoordinate3 *coords, Mesh &he) {
	if (!coords || !faceset) return false;
	coords->point.setNum(0);
	int j = 0;
	for (vector<float>::iterator p=he.verts.begin(); p!=he.verts.end(); p+=3) {
		coords->point.set1Value(j++, *p, *(p+1), *(p+2));
	}

	// read half edges into face set
	faceset->coordIndex.setNum(0);
	// loop over faces
	int i = 0;
	for (vector<HE_face *>::iterator p=he.faces.begin(); p!=he.faces.end(); p++) {
		// loop over edges
		HE_edge *edge = (*p)->edge;
#if SEND_TRIANGLES_INSTEAD_OF_FULL_FACES
		for (int j=0;j<(*p)->edge->face->size-2; j++) {
			HE_edge *cedge = edge;
			for (int k=0; k<3; k++) {
				faceset->coordIndex.set1Value(i++, cedge->vert);
				cedge = cedge->next;
			}
			faceset->coordIndex.set1Value(i++, -1);
			edge = edge->next->next;
		}
		int idx = p-he.faces.begin();
		int z = ((*p)->type==HE_face::dart?0:((*p)->type==HE_face::kite?1: ((*p)->type==HE_face::thick?2: 3)));
		faceset->materialIndex.set1Value(idx*2,   z);
		faceset->materialIndex.set1Value(idx*2+1, z);
#else
		do {
			faceset->coordIndex.set1Value(i++, edge->vert);
			edge = edge->next;
		} while (edge != (*p)->edge);
		faceset->coordIndex.set1Value(i++, -1);
		int idx = p-he.faces.begin();
		faceset->materialIndex.set1Value(idx,   ((*p)->type==HE_face::dart?0:((*p)->type==HE_face::kite?1: ((*p)->type==HE_face::thick?2: 3))));
#endif
	}
	cerr << "total curv: " << he.total_curvature() << endl;
	return true;
}


float Mesh::total_curvature() {
	vector<bool> vmark(verts.size(), false);
	float total_curv = 0;
	int cnt = 0;
	for (vector<HE_face *>::iterator p=faces.begin(); p!=faces.end(); p++) {
		HE_face *face = *p;
		HE_edge *fedge = face->tip;
		do {
			if (!vmark[fedge->vert]) {
			float curv = -2*M_PI;
			bool allsame = true;
			HE_edge *edge = fedge;

			do {
				if (edge->pair) {
					edge = edge->pair->prev;
				} else {
					allsame = false;
					vmark[fedge->vert] = true;
					break;
				}
			} while (edge!=fedge);
			if (!allsame) continue;
			edge = fedge;
			do {
				Vector3f v[3];
				for (int i=0; i<3; i++) {
					v[0].raw[i] = verts[edge->prev->vert*3+i];
					v[1].raw[i] = verts[edge->vert*3+i];
					v[2].raw[i] = verts[edge->next->vert*3+i];
				}
				//signed_angle = atan2(  N * ( V1 x V2 ), V1 * V2  );
				// where * is dot product and x is cross product
				// N is the normal to the polygon
				// ALL vectors: N, V1, V2 must be normalized
				Vector3f V1 = v[0]-v[1];
				Vector3f V2 = v[2]-v[1];
				Vector3f N  = V1^V2;
				N.normalize();
				V1.normalize();
				V2.normalize();
				float signed_angle = atan2(N * ( V1^V2 ), V1 * V2);
				curv += signed_angle;
				//			cerr << edge->vert << " " << signed_angle*180/M_PI << endl;
				edge = edge->pair->prev;
			} while (edge!=fedge);
			total_curv += fabs(curv);
			cnt++;
			vmark[fedge->vert] = true;
			}
			fedge = fedge->next;
		} while (fedge!=face->tip);
	}
//	cerr << "total curv: " << total_curv << " in " << cnt << " verts" << endl;
	return total_curv;
}

